﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using OpenNI;
using MathNet.Numerics.LinearAlgebra;
using BTKinect.DataType;
using BTKinect.DataIO;
using System.ComponentModel;

namespace BTKinect
{
    class KinectOptimiseData
    {
        #region MEMBERS
        public KinectOptimiseData(KinectData k, DepthGenerator d)
        {
            kd = k;
            kbc = new KinectBasicCalculations(d);
        }
        public KinectOptimiseData(KinectData k, DepthGenerator d, BackgroundWorker bg)
        {
            kd = k;
            kbc = new KinectBasicCalculations(d);
            BG = bg;
        }
        private SingularValueDecomposition svd;
        private KinectData kd;
        private KinectBasicCalculations kbc;
        private KinectSelectDesiredData ksd;
        private double[] averageLength;
        private BackgroundWorker BG = null;
        private int _Progress = 0;
        #endregion

        #region METHODS
        private KinectVector calculateGeometrixCentroid(KinectMatrix matrix)
        {
            //tmp var
            int tmpRowCount = matrix.RowCount;
            int tmpColumnCount = matrix.ColumnCount;
            KinectVector[] tmpVector = new KinectVector[tmpColumnCount];
            double[] tmpDouble = new double[tmpRowCount];


            //calculate centroid
            for (int i = 0; i < tmpRowCount; i++)
            {
                tmpVector[i] = new KinectVector(matrix.GetRowVector(i));
                try
                {
                    tmpDouble[i] = (tmpVector[i].Sum() / matrix.ColumnCount);
                }
                catch (Exception)
                {
                    tmpDouble[i] = 0; //the eleventh commandment: thou shall not divide by zero
                }
            }

            return new KinectVector(tmpDouble);
        }
        private KinectMatrix createArtificialMatrix(float lengthSeg1, float lengthSeg2, double angleDegrees)
        {
            //tmp var
            KinectPoint3D pntA = new KinectPoint3D();
            KinectPoint3D pntB = new KinectPoint3D();
            KinectPoint3D pntC = new KinectPoint3D();
            float angleRadian = kbc.DegreeToRadian(180 - angleDegrees);

            //create pntA, aligned with the x-axes
            pntA.X = -lengthSeg1;
            pntA.Y = 0;
            pntA.Z = 0;

            //creat pntB,
            pntB.X = 0;
            pntB.Y = 0;
            pntB.Z = 0;

            //create pntC, 
            pntC.X = (float)(Math.Cos(angleRadian) * lengthSeg2);
            pntC.Y = (float)(Math.Sin(angleRadian) * lengthSeg2);
            pntC.Z = 0;

            //create matrix
            return new KinectMatrix(pntA, pntB, pntC);
        }
        public KinectData OptimizeJointData()
        {
            if (BG == null)
                return optimizeJointData();
            else
                return optimizeJointData(true);
        }
        private KinectData optimizeJointData()
        {
            ksd = new KinectSelectDesiredData(kd);

            //filter out required data
            foreach (var sk in kd.SkeletonJointsToBeOptimised)
            {
                switch (sk)
                {
                    case SkeletonJoint.LeftElbow:
                        kd.OptimisedElbowLeft = ksd.GetJointsData(SkeletonJoint.LeftShoulder, sk, SkeletonJoint.LeftHand);
                        kd.OptimisedElbowLeft = findOptimizedData(kd.OptimisedElbowLeft);
                        break;
                    case SkeletonJoint.LeftKnee:
                        kd.OptimisedKneeLeft = ksd.GetJointsData(SkeletonJoint.LeftHip, sk, SkeletonJoint.LeftFoot);
                        kd.OptimisedKneeLeft = findOptimizedData(kd.OptimisedKneeLeft);
                        break;
                    case SkeletonJoint.LeftShoulder:
                        kd.OptimisedShoulderLeft = ksd.GetJointsData(SkeletonJoint.RightShoulder, sk, SkeletonJoint.LeftElbow);
                        kd.OptimisedShoulderLeft = findOptimizedData(kd.OptimisedShoulderLeft);
                        break;
                    case SkeletonJoint.RightElbow:
                        kd.OptimisedElbowRight = ksd.GetJointsData(SkeletonJoint.RightShoulder, sk, SkeletonJoint.RightHand);
                        kd.OptimisedElbowRight = findOptimizedData(kd.OptimisedElbowRight);
                        break;
                    case SkeletonJoint.RightKnee:
                        kd.OptimisedKneeRight = ksd.GetJointsData(SkeletonJoint.RightHip, sk, SkeletonJoint.RightHand);
                        kd.OptimisedKneeRight = findOptimizedData(kd.OptimisedKneeRight);
                        break;
                    case SkeletonJoint.RightShoulder:
                        kd.OptimisedShoulderRight = ksd.GetJointsData(SkeletonJoint.LeftShoulder, sk, SkeletonJoint.RightElbow);
                        kd.OptimisedShoulderRight = findOptimizedData(kd.OptimisedShoulderRight);
                        break;
                    default:
                        throw new System.ArgumentException("No valid joint, choose from Elbow, Knee and/or shoulder", "original");
                }
            }
            return kd;
        }
        private KinectData optimizeJointData(bool backGroundWorkerPresent)
        {
            ksd = new KinectSelectDesiredData(kd);
            BG.ReportProgress(2);
            float teller = 0;
            //filter out required data
            foreach (var sk in kd.SkeletonJointsToBeOptimised)
            {
                switch (sk)
                {
                    case SkeletonJoint.LeftElbow:
                        kd.OptimisedElbowLeft = ksd.GetJointsData(SkeletonJoint.LeftShoulder, sk, SkeletonJoint.LeftHand);
                        kd.OptimisedElbowLeft = findOptimizedData(kd.OptimisedElbowLeft);
                        break;
                    case SkeletonJoint.LeftKnee:
                        kd.OptimisedKneeLeft = ksd.GetJointsData(SkeletonJoint.LeftHip, sk, SkeletonJoint.LeftFoot);
                        kd.OptimisedKneeLeft = findOptimizedData(kd.OptimisedKneeLeft);
                        break;
                    case SkeletonJoint.LeftShoulder:
                        kd.OptimisedShoulderLeft = ksd.GetJointsData(SkeletonJoint.RightShoulder, sk, SkeletonJoint.LeftElbow);
                        kd.OptimisedShoulderLeft = findOptimizedData(kd.OptimisedShoulderLeft);
                        break;
                    case SkeletonJoint.RightElbow:
                        kd.OptimisedElbowRight = ksd.GetJointsData(SkeletonJoint.RightShoulder, sk, SkeletonJoint.RightHand);
                        kd.OptimisedElbowRight = findOptimizedData(kd.OptimisedElbowRight);
                        break;
                    case SkeletonJoint.RightKnee:
                        kd.OptimisedKneeRight = ksd.GetJointsData(SkeletonJoint.RightHip, sk, SkeletonJoint.RightHand);
                        kd.OptimisedKneeRight = findOptimizedData(kd.OptimisedKneeRight);
                        break;
                    case SkeletonJoint.RightShoulder:
                        kd.OptimisedShoulderRight = ksd.GetJointsData(SkeletonJoint.LeftShoulder, sk, SkeletonJoint.RightElbow);
                        kd.OptimisedShoulderRight = findOptimizedData(kd.OptimisedShoulderRight);
                        break;
                    default:
                        throw new System.ArgumentException("No valid joint, choose from Elbow, Knee and/or shoulder", "original");
                }
                _Progress = (int)(++teller / ((float)kd.NumberOfOptimisedDataSets) * 100);
                BG.ReportProgress(_Progress);
            }
            BG.ReportProgress(100);
            return kd;
        }
        private List<KinectPoint3D[]> findOptimizedData(List<KinectPoint3D[]> lstPoints)
        {
            //calculate average length
            averageLength = calculateAverageLength(lstPoints);

            List<KinectPoint3D[]> lst = new List<KinectPoint3D[]>(kd.MaxLengthOptimisedDataset);
            KinectMatrix matrixMeetData;
            KinectVector zwaartePunten;

            
            foreach (var item in lstPoints) //for every frame with 3D data
            {

                //maak matrix van 3d punten                                                   
                matrixMeetData = new KinectMatrix(item[0], item[1], item[2]);

                //zwaartepunt berekenen meetdata                                                
                zwaartePunten = calculateGeometrixCentroid(matrixMeetData);

                Dictionary<int, float> dic;                     //wordt bij elke aanroep van 'findBestMatch' opnieuw geinstantieerd
                Dictionary<float, int> cid;                     //wordt bij elke aanroep van 'findBestMatch' opnieuw geinstantieerd
                List<KinectMatrix> lstWithTheOptimalMatrix;     //wordt bij elke aanroep van 'findBestMatch' opnieuw geinstantieerd

                //-----------------------------------------------------------------------------------------------------------------------
                //put best matches in a list called: lstWithTheOptimalMatrix

                //first search:
                float step1 = 1;
                int k = findBestMatch(matrixMeetData, zwaartePunten, out dic, out cid, out lstWithTheOptimalMatrix, step1, 0 + step1, 180 - step1);

                //second time:
                float t = k;
                float step2 = 0.2f;
                k = findBestMatch(matrixMeetData, zwaartePunten, out dic, out cid, out lstWithTheOptimalMatrix, step2, t - step1, t + step1);
                //-----------------------------------------------------------------------------------------------------------------------

                //geroteerde matrix terugzetten ///VERGETEN! //moet op deze plaats om fouten in keys (cid, dic) te voorkomen
                KinectMatrix m = lstWithTheOptimalMatrix[k] + zwaartePunten;
                //Matrix m = lstWithTheOptimalMatrix[k] + vZwaartePunten;

                //save optimized point in list
                lst.Add((KinectPoint3D[])m.RoundMatrix(0));

                //clear temp Lists
                dic.Clear();
                cid.Clear();
                lstWithTheOptimalMatrix.Clear();
            }

            //return list with optimized points
            return lst;
        }
        private int findBestMatch(KinectMatrix matrixMeetData, KinectVector zwaartePunten, out Dictionary<int, float> dic, out Dictionary<float, int> cid, out List<KinectMatrix> lstWithTheOptimalMatrix, float step, float start, float stop)
        {
            int beginProgress = _Progress;

            //instantieren van variabelen
            KinectVector vec = new KinectVector(3);
            dic = new Dictionary<int, float>(kd.MaxLengthOptimisedDataset);
            cid = new Dictionary<float, int>(kd.MaxLengthOptimisedDataset);
            lstWithTheOptimalMatrix = new List<KinectMatrix>(kd.MaxLengthOptimisedDataset);
            int teller = 0;

            //iterate for given stepsize, startvalue, stopvalue and find the best fitting vector
            for (float i = start; i < stop; i = i + step) //for every angle
            {
                //if a progressbar is given then updat
                if (BG != null)
                    BG.ReportProgress((int)((float)teller / 11.25F) + beginProgress);

                vec = calculateDistanceBetweenArtificialMatrixAndRealMatrix(matrixMeetData, zwaartePunten, i, ref lstWithTheOptimalMatrix);
                float sumVector = (float)vec.Sum();

                //test if key exists, if so change value
                int tmp;
                while (cid.TryGetValue(sumVector, out tmp))
                {
                    sumVector += 0.001F;
                }

                dic.Add(teller, sumVector);
                cid.Add(sumVector, teller);
                teller += 1;
            }

            //find key with the smallest distance
            return cid[dic.Values.Min()];
        }
        private KinectVector calculateDistanceBetweenArtificialMatrixAndRealMatrix(KinectMatrix mMeetData, KinectVector vZwaartePunten, double AngleDegree, ref List<KinectMatrix> lstWithTheOptimalMatrix)
        {
            //a.h.v. zwaartepunt de vector verschuiven naar nul
            KinectMatrix tmpMatrix = mMeetData - vZwaartePunten;

            //kunstmatige matrix creeren
            KinectMatrix tmpKunstmatigeMatrix = createArtificialMatrix((float)averageLength[0], (float)averageLength[0], AngleDegree);
            //zwaartepunten berekenen
            KinectVector tmpKunstmatigeVectorZwaartepunt = calculateGeometrixCentroid(tmpKunstmatigeMatrix);
            //vector terugschuiven naar nul
            tmpKunstmatigeMatrix = tmpKunstmatigeMatrix - tmpKunstmatigeVectorZwaartepunt;

            //creeeren vna matrix Q
            tmpKunstmatigeMatrix.Transpose(); //Heen en
            Matrix Q = tmpMatrix * tmpKunstmatigeMatrix;
            tmpKunstmatigeMatrix.Transpose(); //Weer

            //SVD berekenen
            svd = new SingularValueDecomposition(Q);

            //uitkomst van SVD toekennen
            KinectMatrix T = new KinectMatrix(svd.LeftSingularVectors);
            KinectMatrix U = new KinectMatrix(svd.S);
            KinectMatrix V = new KinectMatrix(svd.RightSingularVectors);

            //tussen berekeningen
            V.Transpose();
            KinectMatrix TV_ = new KinectMatrix(T * V);
            float detTV_ = (float)TV_.Determinant();

            //create matrix to calculate rotation matrix
            Matrix U2 = new Matrix(new double[][] 
            {
            new double[] { 1, 0, 0 },
            new double[] { 0,1, 0 },
            new double[] { 0,0, detTV_}
            });

            //Rotatie matrix
            KinectMatrix R = new KinectMatrix((T * U2 * V)); //V is already transposed

            //geroteerde kunstmatige punten
            KinectMatrix kunstMatigGeroteerd = new KinectMatrix(R * tmpKunstmatigeMatrix);

            lstWithTheOptimalMatrix.Add(kunstMatigGeroteerd);

            return tmpMatrix.Distance(kunstMatigGeroteerd);
        }
        private double[] calculateAverageLength(List<KinectPoint3D[]> lst)
        {
            List<double> lengthSegment1 = new List<double>(lst.Count);
            List<double> lengthSegment2 = new List<double>(lst.Count);

            //get Point3D from list and calculate lengt, put length in list
            foreach (var item in lst)
            {
                lengthSegment1.Add(item[0].Distance(item[1]));
                lengthSegment2.Add(item[1].Distance(item[2]));
            }
            return new double[] { lengthSegment1.Average(), lengthSegment2.Average() };
        }
        #endregion
    }
}
